// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------
// Modification history:
// feature: authentication-free shared key and token derivation between clients
// feature: listener code decoupling
// feature: cmd tool no longer relies on the xml config
// feature: change E2E packet format
// feature: agent for vbs
// feature: DServer support simple entity
// feature: support auth token
// feature: develop dispatcher
// feature: modify log level
// feature: Unified Transport Abstraction Layer Unified Transport Abstraction Layer
// feature: interval report metrics
// feature: crash on exit
// feature: Development of new requirements for E2E functionality
// feature: E2E protection
// ------------------------------------------------------------------

#include <edds/rtps/messages/MessageReceiver.h>

#include <elog/log/Log.h>
#include <edds/rtps/reader/RTPSReader.h>
#include <edds/rtps/writer/RTPSWriter.h>
#include <rtps/participant/RTPSParticipantImpl.h>

#include <cassert>
#include <limits>

#include <edds/dds/core/policy/ParameterList.hpp>
#include <deps/common/shared_mutex.hpp>
#include <xmlparser/XMLParserPart.h>

#if !defined(_WIN32)
#include <tracepoint/TracePointManager.hpp>
#endif

#define INFO_SRC_SUBMSG_LENGTH 20U

// R[0x52] T[0x54] P[0x50] S[0x53]
#define RTPS_HEADER_HEX 0x53505452

#define IDSTRING "(ID:" << std::this_thread::get_id() << ") " <<

using ParameterList = evbs::edds::dds::ParameterList;

namespace evbs {
namespace ertps {
namespace rtps {

MessageReceiver::MessageReceiver(RTPSParticipantImpl* participant, uint32_t rec_buffer_size)
    : participant_(participant),
#if HAVE_SECURITY
      crypto_msg_(participant->is_secure() ? rec_buffer_size : 0U),
      crypto_submsg_(participant->is_secure() ? rec_buffer_size : 0U),
      crypto_payload_(participant->is_secure() ? rec_buffer_size : 0U),
#endif  // if HAVE_SECURITY
      is_compatible(false) {
    (void)rec_buffer_size;

#if HAVE_SECURITY
    if (participant->is_secure()) {
#ifdef BATCH_SEND_ENABLE
        process_data_batch_message_function_ = std::bind(&MessageReceiver::process_data_batch_message_with_security,
                                                         this, std::placeholders::_1, std::placeholders::_2);
#endif
        process_data_message_function_ = std::bind(&MessageReceiver::process_data_message_with_security, this,
                                                   std::placeholders::_1, std::placeholders::_2);

        process_data_fragment_message_function_ =
            std::bind(&MessageReceiver::process_data_fragment_message_with_security, this, std::placeholders::_1,
                      std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5);
    } else {
#endif  // if HAVE SECURITY

#ifdef BATCH_SEND_ENABLE
        process_data_batch_message_function_ = std::bind(&MessageReceiver::process_data_batch_message_without_security,
                                                         this, std::placeholders::_1, std::placeholders::_2);
#endif
        process_data_message_function_ = std::bind(&MessageReceiver::process_data_message_without_security, this,
                                                   std::placeholders::_1, std::placeholders::_2);

        process_data_fragment_message_function_ =
            std::bind(&MessageReceiver::process_data_fragment_message_without_security, this, std::placeholders::_1,
                      std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5);
#if HAVE_SECURITY
    }

#endif  // if HAVE SECURITY
}

MessageReceiver::~MessageReceiver() {
    logDebug(RTPS_MSG_IN, "");
    assert(associated_writers_.empty());
    assert(associated_readers_.empty());
}

#if HAVE_SECURITY
#ifdef BATCH_SEND_ENABLE
void MessageReceiver::process_data_batch_message_with_security(const EntityId_t& reader_id, CacheChange_t& change) {
    auto process_message = [&change, this](RTPSReader* reader) {
        if (!reader->getAttributes().security_attributes().is_payload_protected) {
            reader->processDataBatchMsg(&change);
            return;
        }

        if (!reader->matched_writer_is_matched(change.writerGUID)) {
            return;
        }

        if (!participant_->security_manager().decode_serialized_payload(change.serializedPayload, crypto_payload_,
                                                                        reader->getGuid(), change.writerGUID)) {
            return;
        }

        std::swap(change.serializedPayload.data, crypto_payload_.data);
        std::swap(change.serializedPayload.length, crypto_payload_.length);

        SerializedPayload_t original_payload = change.serializedPayload;
        (void)reader->processDataBatchMsg(&change);
        IPayloadPool* payload_pool = change.payload_owner();
        if (payload_pool) {
            (void)payload_pool->release_payload(change);
            change.serializedPayload = original_payload;
        }
        original_payload.data = nullptr;
        std::swap(change.serializedPayload.data, crypto_payload_.data);
        std::swap(change.serializedPayload.length, crypto_payload_.length);
    };

    findAllReaders(reader_id, process_message);
}
#endif

void MessageReceiver::process_data_message_with_security(const EntityId_t& reader_id, CacheChange_t& change) {
    auto process_message = [&change, this](RTPSReader* reader) {
        MessageLog(reader->getTopicName(), change.writerGUID, MSG_TRACE_TYPE_RECV_IN, change.sequenceNumber.to64long(),
                   change.sourceTimestamp, change.serializedPayload.length);
        if (reader->getGuid().entityId == participant_stateless_message_reader_entity_id &&
            change.writerGUID.entityId == participant_stateless_message_writer_entity_id) {
            std::ostringstream oss;
            oss << change.writerGUID << " " << reader->getGuid();
            elogInfoKeyT(SECURITY, oss.str(),
                         "Process handshake message from " << change.writerGUID << " to " << reader->getGuid()
                                                           << " length " << change.serializedPayload.length);
        }
        if (!reader->getAttributes().security_attributes().is_payload_protected) {
            reader->before_process_data();
            (void)reader->processDataMsg(&change);
            reader->after_process_data();
            return;
        }

        if (!reader->matched_writer_is_matched(change.writerGUID)) {
            return;
        }

        if (!participant_->security_manager().decode_serialized_payload(change.serializedPayload, crypto_payload_,
                                                                        reader->getGuid(), change.writerGUID)) {
            return;
        }

        std::swap(change.serializedPayload.data, crypto_payload_.data);
        std::swap(change.serializedPayload.length, crypto_payload_.length);

        SerializedPayload_t original_payload = change.serializedPayload;
        reader->before_process_data();
        (void)reader->processDataMsg(&change);
        reader->after_process_data();
        IPayloadPool* payload_pool = change.payload_owner();
        if (payload_pool) {
            (void)payload_pool->release_payload(change);
            change.serializedPayload = original_payload;
        }
        original_payload.data = nullptr;
        std::swap(change.serializedPayload.data, crypto_payload_.data);
        std::swap(change.serializedPayload.length, crypto_payload_.length);
    };

    if (get_compatible()) {
        findAllReaders(c_EntityId_Compatible_SPDPReader, process_message);
    } else {
        if (reader_id == participant_stateless_message_reader_entity_id) {
            logInfo(SECURIYT, "Try to find reader for " << reader_id);
        }
        findAllReaders(reader_id, process_message);
    }
}

void MessageReceiver::process_data_fragment_message_with_security(const EntityId_t& reader_id, CacheChange_t& change,
                                                                  uint32_t sample_size, uint32_t fragment_starting_num,
                                                                  uint16_t fragments_in_submessage) {
    auto process_message = [&change, sample_size, fragment_starting_num, fragments_in_submessage,
                            this](RTPSReader* reader) {
        MessageLog(reader->getTopicName(), change.writerGUID, MSG_TRACE_TYPE_RECV_IN, change.sequenceNumber.to64long(),
                   change.sourceTimestamp, change.serializedPayload.length);
        if (reader->getGuid().entityId == participant_stateless_message_reader_entity_id &&
            change.writerGUID.entityId == participant_stateless_message_writer_entity_id) {
            std::ostringstream oss;
            oss << change.writerGUID << " " << reader->getGuid();
            elogInfoKeyT(SECURITY, oss.str(),
                         "Process handshake message(frag) from " << change.writerGUID << " to " << reader->getGuid()
                                                                 << " length " << change.serializedPayload.length);
        }
        if (!reader->getAttributes().security_attributes().is_payload_protected) {
            reader->before_process_data();
            (void)reader->processDataFragMsg(&change, sample_size, fragment_starting_num, fragments_in_submessage);
            reader->after_process_data();
            return;
        }

        if (!reader->matched_writer_is_matched(change.writerGUID)) {
            return;
        }

        if (!participant_->security_manager().decode_serialized_payload(change.serializedPayload, crypto_payload_,
                                                                        reader->getGuid(), change.writerGUID)) {
            return;
        }

        std::swap(change.serializedPayload.data, crypto_payload_.data);
        std::swap(change.serializedPayload.length, crypto_payload_.length);
        reader->before_process_data();
        (void)reader->processDataFragMsg(&change, sample_size, fragment_starting_num, fragments_in_submessage);
        reader->after_process_data();
        std::swap(change.serializedPayload.data, crypto_payload_.data);
        std::swap(change.serializedPayload.length, crypto_payload_.length);
    };

    findAllReaders(reader_id, process_message);
}

#endif  // if HAVE SECURITY

#ifdef BATCH_SEND_ENABLE
void MessageReceiver::process_data_batch_message_without_security(const EntityId_t& reader_id, CacheChange_t& change) {
    auto process_message = [&change](RTPSReader* reader) {
        reader->processDataBatchMsg(&change);
    };

    findAllReaders(reader_id, process_message);
}
#endif

void MessageReceiver::process_data_message_without_security(const EntityId_t& reader_id, CacheChange_t& change) {
    auto process_message = [&change](RTPSReader* reader) {
        MessageLog(reader->getTopicName(), change.writerGUID, MSG_TRACE_TYPE_RECV_IN, change.sequenceNumber.to64long(),
                   change.sourceTimestamp, change.serializedPayload.length);
        logDebug(RTPS_MSG_IN, "find reader " << change.readerGUID);
        reader->before_process_data();
        (void)reader->processDataMsg(&change);
        reader->after_process_data();
    };

    if (reader_id == c_EntityId_Unknown) {
        if (change.writerGUID.entityId == c_EntityId_SPDPWriter) {
            // 假如为兼容PDP报文通过兼容Reader收取c_EntityId_Compatible_SPDPReader
            if (get_compatible()) {
                findAllReaders(c_EntityId_Compatible_SPDPReader, process_message);
            } else {
                findAllReaders(c_EntityId_SPDPReader, process_message);
            }
            return;
        }
        if (change.writerGUID.entityId == c_EntityId_SEDPPubWriter) {
            findAllReaders(c_EntityId_SEDPPubReader, process_message);
            return;
        }
        if (change.writerGUID.entityId == c_EntityId_SEDPSubWriter) {
            findAllReaders(c_EntityId_SEDPSubReader, process_message);
            return;
        }
    }

    if (get_compatible()) {
        findAllReaders(c_EntityId_Compatible_SPDPReader, process_message);
    } else {
        findAllReaders(reader_id, process_message);
    }
}

void MessageReceiver::process_data_fragment_message_without_security(const EntityId_t& reader_id, CacheChange_t& change,
                                                                     uint32_t sample_size,
                                                                     uint32_t fragment_starting_num,
                                                                     uint16_t fragments_in_submessage) {
    auto process_message = [&change, sample_size, fragment_starting_num, fragments_in_submessage](RTPSReader* reader) {
        MessageLog(reader->getTopicName(), change.writerGUID, MSG_TRACE_TYPE_RECV_IN, change.sequenceNumber.to64long(),
                   change.sourceTimestamp, change.serializedPayload.length);
        reader->before_process_data();
        (void)reader->processDataFragMsg(&change, sample_size, fragment_starting_num, fragments_in_submessage);
        reader->after_process_data();
    };

    findAllReaders(reader_id, process_message);
}

void MessageReceiver::associateEndpoint(Endpoint* to_add) {
    std::lock_guard<vbs::shared_mutex> guard(mtx_);

    if (to_add == nullptr) {
        return;
    }

    if (to_add->getAttributes().endpointKind == WRITER) {
        const auto writer = dynamic_cast<RTPSWriter*>(to_add);
        for (const auto& it : associated_writers_) {
            if (it == writer) {
                return;
            }
        }

        associated_writers_.push_back(writer);
    } else {
        const auto reader = dynamic_cast<RTPSReader*>(to_add);
        if (reader == nullptr) {
            return;
        }
        const auto entityId = reader->getGuid().entityId;
        // search for set of readers by entity ID
        const auto readers = associated_readers_.find(entityId);
        if (readers == associated_readers_.end()) {
            auto vec = std::vector<RTPSReader*>();
            vec.push_back(reader);
            (void)associated_readers_.emplace(entityId, vec);
        } else {
            for (const auto& it : readers->second) {
                if (it == reader) {
                    return;
                }
            }

            readers->second.push_back(reader);
        }
    }
}

void MessageReceiver::removeEndpoint(Endpoint* to_remove) {
    std::lock_guard<vbs::shared_mutex> guard(mtx_);

    if (to_remove->getAttributes().endpointKind == WRITER) {
        auto* var = dynamic_cast<RTPSWriter*>(to_remove);
        for (auto it = associated_writers_.begin(); it != associated_writers_.end(); ++it) {
            if (*it == var) {
                (void)associated_writers_.erase(it);
                break;
            }
        }
    } else {
        auto readers = associated_readers_.find(to_remove->getGuid().entityId);
        if (readers != associated_readers_.end()) {
            auto* var = dynamic_cast<RTPSReader*>(to_remove);
            for (auto it = readers->second.begin(); it != readers->second.end(); ++it) {
                if (*it == var) {
                    (void)readers->second.erase(it);
                    if (readers->second.empty()) {
                        (void)associated_readers_.erase(readers);
                    }
                    break;
                }
            }
        }
    }
}

void MessageReceiver::set_compatible() {
    is_compatible = true;
}

bool MessageReceiver::get_compatible() const {
    return is_compatible;
}

inline void MessageReceiver::reset() {}

void MessageReceiver::processMsg(const ertps::rtps::octet* data, const uint32_t size,
                                 const Locator_t& reception_locator, const Locator_t& remote_locator) {
    logDebug(RTPS_MSG_IN, "processMsg  msg " << size);

    CDRMessage_t* pmsg;

    CDRMessage_t msg(0U);
    msg.wraps = true;
    msg.buffer = const_cast<octet*>(data);
    msg.length = size;
    msg.max_size = size;
    msg.reserved_size = size;

    if (msg.length < RTPSMESSAGE_HEADER_SIZE) {
        logDebug(RTPS_MSG_IN, IDSTRING "Received message too short, ignoring");
        return;
    }

#ifdef FUZZING_BUILD_MODE_UNSAFE_FOR_PRODUCTION
    GuidPrefix_t participantGuidPrefix;
#else
    GuidPrefix_t participantGuidPrefix = participant_->getGuid().guidPrefix;
#endif  // ifdef FUZZING_BUILD_MODE_UNSAFE_FOR_PRODUCTION

#if HAVE_SECURITY
    security::SecurityManager& security = participant_->security_manager();
    CDRMessage_t* auxiliary_buffer = &crypto_msg_;
    int decode_ret = 0;
#endif  // if HAVE_SECURITY

    pmsg = &msg;
    struct MessageInfo msg_info;
    {
        std::lock_guard<vbs::shared_mutex> guard(mtx_);

        reset();

        msg_info.dest_guid_prefix_ = participantGuidPrefix;

        pmsg->pos = 0;  // Start reading at 0

        // Once everything is set, the reading begins:
        if (!checkRTPSHeader(pmsg, msg_info)) {
            return;
        }
#if !defined(WIN32)
        msg_info.remote_locator = remote_locator;
#endif
        msg_info.reception_locator = reception_locator;
#if HAVE_SECURITY
        decode_ret = security.decode_rtps_message(*pmsg, *auxiliary_buffer, msg_info.source_guid_prefix_);

        if (decode_ret < 0) {
            return;
        }

        if (decode_ret == 0) {
            // The original CDRMessage buffer (msg) now points to the proprietary temporary buffer
            // crypto_msg_. The auxiliary buffer now points to the propietary temporary buffer
            // crypto_submsg_. This way each decoded sub-message will be processed using the
            // crypto_submsg_ buffer.
            pmsg = auxiliary_buffer;
            auxiliary_buffer = &crypto_submsg_;
        }
#endif  // if HAVE_SECURITY
    }
#if HAVE_SECURITY
    if (security.authTokenAlwaysOn || participant_->is_secure()) {
        if (msg_info.source_guid_prefix_.value[0] != c_VendorId_vbs[0]) {
            msg_info.must_check_ = false;
        } else {
            const EvbsVersion_t version = participant_->get_version_by_guid(msg_info.source_guid_prefix_);
            if (version.empty() || version == evbs::edds::rtps::c_Version_vbs_unknown) {
                msg_info.must_check_ = false;
            } else if (version[0] < 2) {
                msg_info.must_check_ = false;
            }
        }
    }
#endif

    // Loop until there are no more submessages
    // Each submessage processing method choses the lock kind required
    bool valid;
    SubmessageHeader_t submsgh;  // Current submessage header

    while (pmsg->pos < pmsg->length)  // end of the message
    {
        CDRMessage_t* submessage = pmsg;

#if HAVE_SECURITY
        decode_ret = security.decode_rtps_submessage(*pmsg, *auxiliary_buffer, msg_info.source_guid_prefix_);

        if (decode_ret < 0) {
            return;
        }

        if (decode_ret == 0) {
            submessage = auxiliary_buffer;
        }
#endif  // if HAVE_SECURITY

        // First 4 bytes must contain: ID | flags | octets to next header
        if (!readSubmessageHeader(submessage, &submsgh)) {
            return;
        }

        valid = true;
        uint32_t next_msg_pos = submessage->pos;
        next_msg_pos += (submsgh.submessageLength + 3u) & ~3u;
        switch (submsgh.submessageId) {
            case DATA: {
                if (msg_info.dest_guid_prefix_ == participantGuidPrefix) {
                    valid = proc_Submsg_Data(submessage, &submsgh, msg_info);
                }
                break;
            }
#ifdef BATCH_SEND_ENABLE
            case DATA_BATCH:
                if (msg_info.dest_guid_prefix_ == participantGuidPrefix) {
                    valid = proc_Submsg_DataBatch(submessage, &submsgh, msg_info);
                }
                break;
#endif
            case DATA_FRAG:
                if (msg_info.dest_guid_prefix_ == participantGuidPrefix) {
                    valid = proc_Submsg_DataFrag(submessage, &submsgh, msg_info);
                }
                break;
            case GAP: {
                if (msg_info.dest_guid_prefix_ == participantGuidPrefix) {
                    valid = proc_Submsg_Gap(submessage, &submsgh, msg_info);
                }
                break;
            }
            case ACKNACK: {
                if (msg_info.dest_guid_prefix_ == participantGuidPrefix) {
                    valid = proc_Submsg_Acknack(submessage, &submsgh, msg_info);
                }
                break;
            }
            case NACK_FRAG: {
                if (msg_info.dest_guid_prefix_ == participantGuidPrefix) {
                    valid = proc_Submsg_NackFrag(submessage, &submsgh, msg_info);
                }
                break;
            }
            case HEARTBEAT: {
                if (msg_info.dest_guid_prefix_ == participantGuidPrefix) {
                    valid = proc_Submsg_Heartbeat(submessage, &submsgh, msg_info);
                }
                break;
            }
            case HEARTBEAT_FRAG: {
                if (msg_info.dest_guid_prefix_ == participantGuidPrefix) {
                    valid = proc_Submsg_HeartbeatFrag(submessage, &submsgh, msg_info);
                }
                break;
            }
            case PAD:
                logDebug(RTPS_MSG_IN, IDSTRING "PAD messages not yet implemented, ignoring");
                break;
            case INFO_DST:
                valid = proc_Submsg_InfoDST(submessage, &submsgh, msg_info);
                break;
            case INFO_SRC:
                valid = proc_Submsg_InfoSRC(submessage, &submsgh, msg_info);
                break;
            case INFO_TS: {
                valid = proc_Submsg_InfoTS(submessage, &submsgh, msg_info);
                break;
            }
            case INFO_REPLY:
                break;
            case INFO_REPLY_IP4:
                break;
            case INFO_E2E:
                valid = proc_Submsg_InfoE2E(submessage, &submsgh, msg_info);
                break;
#if HAVE_SECURITY
            case AUTH_TOKEN: {
                valid = proc_Submsg_AuthToken(submessage, &submsgh, msg_info);
                break;
            }
            case SHARED_KEY: {
                valid = proc_Submsg_SharedKey(submessage, &submsgh, msg_info);
                break;
            }
#endif
            default:
                break;
        }

        if ((!valid) || submsgh.is_last) {
            break;
        }

        submessage->pos = next_msg_pos;
    }

#ifndef FUZZING_BUILD_MODE_UNSAFE_FOR_PRODUCTION
    participant_->assert_remote_participant_liveliness(msg_info.source_guid_prefix_);
#endif  // ifndef FUZZING_BUILD_MODE_UNSAFE_FOR_PRODUCTION
}

bool MessageReceiver::checkRTPSHeader(CDRMessage_t* msg, struct MessageInfo& msg_info) {
    // check and proccess the RTPS Header
    if (*reinterpret_cast<uint32_t*>(msg->buffer) != RTPS_HEADER_HEX) {
        return false;
    }

    msg->pos += 4U;

    // CHECK AND SET protocol version
    if (msg->buffer[msg->pos] == c_ProtocolVersion.m_major) {
        msg_info.source_version_.m_major = msg->buffer[msg->pos];
        msg->pos++;
        msg_info.source_version_.m_minor = msg->buffer[msg->pos];
        msg->pos++;
    } else {
        logDebug(RTPS_MSG_IN, IDSTRING "Major RTPS Version not supported");
        return false;
    }

    // Set source vendor id
    msg_info.source_vendor_id_[0] = msg->buffer[msg->pos];
    msg->pos++;
    msg_info.source_vendor_id_[1] = msg->buffer[msg->pos];
    msg->pos++;
    // set source guid prefix
    (void)CDRMessage::readData(msg, msg_info.source_guid_prefix_.value, GuidPrefix_t::size);
    return true;
}

bool MessageReceiver::readSubmessageHeader(CDRMessage_t* msg, SubmessageHeader_t* smh) const {
    if (msg->length - msg->pos < 4U) {
        return false;
    }
    smh->submessageId = msg->buffer[msg->pos];
    msg->pos++;
    smh->flags = msg->buffer[msg->pos];
    msg->pos++;

    // Set endianness of message
    msg->msg_endian = (smh->flags & BIT(0)) != 0U ? LITTLEEND : BIGEND;
    uint16_t length = 0U;
    (void)CDRMessage::readUInt16(msg, &length);
    if (msg->pos + length > msg->length) {
        logDebug(RTPS_MSG_IN, IDSTRING "SubMsg of invalid length (" << length << ") with current msg position/length ("
                                                                    << msg->pos << "/" << msg->length << ")");
        return false;
    }

    if ((length == 0U) && (smh->submessageId != INFO_TS) && (smh->submessageId != PAD)) {
        // THIS IS THE LAST SUBMESSAGE
        smh->submessageLength = msg->length - msg->pos;
        smh->is_last = true;
    } else {
        smh->submessageLength = length;
        smh->is_last = false;
    }

    return true;
}

bool MessageReceiver::willAReaderAcceptMsgDirectedTo(const EntityId_t& readerID, RTPSReader*& first_reader) const {
    first_reader = nullptr;
    if (associated_readers_.empty()) {
        return false;
    }

    if (readerID != c_EntityId_Unknown) {
        const auto readers = associated_readers_.find(readerID);
        if (readers != associated_readers_.end()) {
            first_reader = readers->second.front();
            return true;
        }
    } else {
        for (const auto& readers : associated_readers_) {
            for (const auto& it : readers.second) {
                if (it && it->m_acceptMessagesToUnknownReaders) {
                    first_reader = it;
                    return true;
                }
            }
        }
    }

    return false;
}

template <typename Functor>
void MessageReceiver::findAllReaders(const EntityId_t& readerID, const Functor& callback) const {
    if (readerID != c_EntityId_Unknown) {
        const auto readers = associated_readers_.find(readerID);
        if (readers != associated_readers_.end()) {
            for (const auto& it : readers->second) {
                if (it != nullptr) {
                    callback(it);
                }
            }
        }
    } else {
        for (const auto& readers : associated_readers_) {
            for (const auto& it : readers.second) {
                if (it && it->m_acceptMessagesToUnknownReaders) {
                    callback(it);
                }
            }
        }
    }
}

#if HAVE_SECURITY
bool MessageReceiver::checkAuthToken(const GUID_t& fromGUID, const GUID_t& toGUID, struct MessageInfo& msg_info,
                                     uint64_t seq) const {
    if (!participant_->security_manager().authTokenAlwaysOn && !participant_->is_secure()) {
        return true;
    }

    if (!msg_info.must_check_ || fromGUID.is_builtin() || fromGUID.guidPrefix.value[0] != c_VendorId_vbs[0]) {
        return true;
    }

    // green light or not
    const bool permissive = !vbsutil::xmlparser::XMLParserpart::is_enforcing();
    bool ret =
        participant_->security_manager().checkAuthToken(fromGUID, toGUID, seq, msg_info.auth_token_) || permissive;
    return ret;
}
#endif

bool MessageReceiver::proc_Submsg_Data(CDRMessage_t* msg, SubmessageHeader_t* smh, struct MessageInfo& msg_info) const {
    vbs::shared_lock<vbs::shared_mutex> guard(mtx_);

    // READ and PROCESS
    if (smh->submessageLength < RTPSMESSAGE_DATA_MIN_LENGTH) {
        return false;
    }
    // Fill flags bool values
    bool endiannessFlag = (smh->flags & BIT(0)) != 0U;
    bool inlineQosFlag = (smh->flags & BIT(1)) != 0U;
    bool dataFlag = (smh->flags & BIT(2)) != 0U;
    bool keyFlag = (smh->flags & BIT(3)) != 0U;
    if (keyFlag && dataFlag) {
        logInfo(RTPS_MSG_IN, IDSTRING "Message received with Data and Key Flag set, ignoring");
        return false;
    }

    // Assign message endianness
    if (endiannessFlag) {
        msg->msg_endian = LITTLEEND;
    } else {
        msg->msg_endian = BIGEND;
    }

    bool valid = true;

    // Extra flags don't matter now. Avoid those bytes
    msg->pos += 2U;

    int16_t octetsToInlineQos = 0;
    valid &= CDRMessage::readInt16(msg, &octetsToInlineQos);  // it should be 16 in this implementation

    CacheChange_t ch;
    // reader and writer ID
    RTPSReader* first_reader = nullptr;
    valid &= CDRMessage::readEntityId(msg, &ch.readerGUID.entityId);

    // WE KNOW THE READER THAT THE MESSAGE IS DIRECTED TO SO WE LOOK FOR IT:
    if (!get_compatible() && !willAReaderAcceptMsgDirectedTo(ch.readerGUID.entityId, first_reader)) {
        elogInfoKeyT(RTPS_MSG_IN, std::to_string(ch.readerGUID.entityId.to_uint32()),
                     "Process submsg data error(no reader) from " << ch.writerGUID << " to " << ch.readerGUID);
        return false;
    }

    // FOUND THE READER.
    // We ask the reader for a cachechange to store the information.
    ch.kind = ALIVE;
    ch.writerGUID.guidPrefix = msg_info.source_guid_prefix_;

    if (msg_info.reception_locator.port > UINT16_MAX) {
        logWarning(RTPS_MSG_IN, "Process dst port : " << msg_info.reception_locator.port << "error from "
                                                      << ch.writerGUID << " to " << ch.readerGUID);
    }
    ch.dst_port = static_cast<uint16_t>(msg_info.reception_locator.port);

    valid &= CDRMessage::readEntityId(msg, &ch.writerGUID.entityId);

    // Get sequence number
    valid &= CDRMessage::readSequenceNumber(msg, &ch.sequenceNumber);
    if (!valid) {
        return false;
    }

    if (ch.sequenceNumber <= SequenceNumber_t()) {
        logInfo(RTPS_MSG_IN, IDSTRING "Invalid message received, bad sequence Number");
        return false;
    }
#if HAVE_SECURITY
    GUID_t readerGUID;
    readerGUID.guidPrefix = msg_info.dest_guid_prefix_;
    if (!checkAuthToken(ch.writerGUID, readerGUID, msg_info, ch.sequenceNumber.to64long())) {
        return false;
    }
#endif

    uint32_t inlineQosSize = 0U;
    if (inlineQosFlag) {
        if (!ParameterList::updateCacheChangeFromInlineQos(ch, msg, inlineQosSize)) {
            return false;
        }
        ch.inline_qos.data = &msg->buffer[msg->pos - inlineQosSize];
        ch.inline_qos.max_size = inlineQosSize;
        ch.inline_qos.length = inlineQosSize;
        ch.inline_qos.encapsulation = endiannessFlag ? PL_CDR_LE : PL_CDR_BE;
        ch.inline_qos.pos = 0;
    }

    if (dataFlag || keyFlag) {
        uint32_t payload_size;

        const uint32_t submsg_no_payload_size =
            RTPSMESSAGE_DATA_EXTRA_INLINEQOS_SIZE + octetsToInlineQos + inlineQosSize;

        // Prevent integer overflow of variable payload_size
        if (smh->submessageLength < submsg_no_payload_size) {
            logWarning(RTPS_MSG_IN, IDSTRING
                                        "Serialized Payload avoided overflow "
                                        "("
                                        << smh->submessageLength << "/" << submsg_no_payload_size << ")");
            ch.serializedPayload.data = nullptr;
            ch.inline_qos.data = nullptr;
            return false;
        }

        payload_size = smh->submessageLength - submsg_no_payload_size;

        if (dataFlag) {
            uint32_t next_pos = msg->pos + payload_size;
            if (msg->length >= next_pos && payload_size > 0) {
                ch.serializedPayload.data = &msg->buffer[msg->pos];
                ch.serializedPayload.length = payload_size;
                ch.serializedPayload.max_size = payload_size;
                CDRMessage_t tmp(0);
                tmp.wraps = true;
                tmp.msg_endian = LITTLEEND;
                tmp.length = ch.serializedPayload.length;
                tmp.max_size = ch.serializedPayload.max_size;
                tmp.buffer = ch.serializedPayload.data;
                if (ParameterList::read_guid_from_cdr_msg(tmp, edds::dds::PID_PARTICIPANT_GUID,
                                                          msg_info.matched_peer_)) {
                    logDebug(RTPS_MSG_IN, "wanted matched src:" << msg_info.matched_peer_);
                }
                msg->pos = next_pos;
            } else {
                logWarning(RTPS_MSG_IN, IDSTRING
                                            "Serialized Payload value invalid or larger than maximum allowed size"
                                            "("
                                            << payload_size << "/" << (msg->length - msg->pos) << ")");
                ch.serializedPayload.data = nullptr;
                ch.inline_qos.data = nullptr;
                return false;
            }
        } else if (keyFlag) {
            if (payload_size <= 0U) {
                logWarning(RTPS_MSG_IN, IDSTRING "Serialized Payload value invalid (" << payload_size << ")");
                ch.serializedPayload.data = nullptr;
                ch.inline_qos.data = nullptr;
                return false;
            }
            /*hanjie: key message size=
             * PARAMETER_STATUS_SIZE+PARAMETER_KEY_SIZE+PARAMETER_SENTINEL_SIZE+2+2*/
            if (payload_size <= 36) {
                (void)memcpy(ch.instanceHandle.value, &msg->buffer[msg->pos], payload_size);
                ch.serializedPayload.data = &msg->buffer[msg->pos];
                ch.serializedPayload.length = payload_size;
                ch.serializedPayload.max_size = payload_size;
                msg_info.matched_peer_ = static_cast<const GUID_t&>(ch.instanceHandle);
            } else {
                logWarning(RTPS_MSG_IN,
                           IDSTRING "Ignoring Serialized Payload for too large key-only data (" << payload_size << ")");
            }
            msg->pos += payload_size;
        }
    }

    // Set sourcetimestamp
    if (msg_info.have_timestamp_) {
        ch.sourceTimestamp = msg_info.timestamp_;
    }

    ch.e2e_header = msg_info.e2e_profile04_;

#if !defined(WIN32)
    auto remote_addr = msg_info.remote_locator.address;
    uint32_t remote_ip = static_cast<uint32_t>(remote_addr[12]) | (static_cast<uint32_t>(remote_addr[13]) << 8) |
                         (static_cast<uint32_t>(remote_addr[14]) << 16) |
                         (static_cast<uint32_t>(remote_addr[15]) << 24);
    uint32_t dst_ip = 0, mask_len = 32;
    if (vbsutil::tracepoint::TracePointManager::find_same_subnet_ip(remote_ip, dst_ip, mask_len)) {
        ch.src_ip = remote_ip;
        ch.dst_ip = dst_ip;
        ch.mask_len = mask_len;
    }
#endif

    // Look for the correct reader to add the change
    process_data_message_function_(ch.readerGUID.entityId, ch);

    IPayloadPool* payload_pool = ch.payload_owner();
    if (payload_pool) {
        (void)payload_pool->release_payload(ch);
    }

    // TODO(Ricardo) If an exception is thrown (ex, by ecdr), these lines are not executed ->
    // segmentation fault
    ch.serializedPayload.data = nullptr;
    ch.inline_qos.data = nullptr;
    return true;
}

bool MessageReceiver::proc_Submsg_DataBatch(CDRMessage_t* msg, SubmessageHeader_t* smh, struct MessageInfo& msg_info) {
#ifdef BATCH_SEND_ENABLE
    vbs::shared_lock<vbs::shared_mutex> guard(mtx_);

    // READ and PROCESS
    if (smh->submessageLength < RTPSMESSAGE_DATA_MIN_LENGTH) {
        return false;
    }
    // Fill flags bool values
    bool endiannessFlag = (smh->flags & BIT(0)) != 0U;
    bool inlineQosFlag = (smh->flags & BIT(1)) != 0U;
    bool keyFlag = (smh->flags & BIT(3)) != 0U;

    // Assign message endianness
    if (endiannessFlag) {
        msg->msg_endian = LITTLEEND;
    } else {
        msg->msg_endian = BIGEND;
    }

    bool valid = true;

    // Extra flags don't matter now. Avoid those bytes
    msg->pos += 2U;

    int16_t octetsToInlineQos = 0;
    valid &= CDRMessage::readInt16(msg, &octetsToInlineQos);  // it should be 16 in this implementation

    CacheChange_t ch;
    // reader and writer ID
    RTPSReader* first_reader = nullptr;
    valid &= CDRMessage::readEntityId(msg, &ch.readerGUID.entityId);

    // WE KNOW THE READER THAT THE MESSAGE IS DIRECTED TO SO WE LOOK FOR IT:
    if (!willAReaderAcceptMsgDirectedTo(ch.readerGUID.entityId, first_reader)) {
        logWarning(RTPS_MSG_IN,
                   "Process submsg data error(no reader) from " << ch.writerGUID << " to " << ch.readerGUID);
        return false;
    }

    // FOUND THE READER.
    // We ask the reader for a cachechange to store the information.
    ch.kind = ALIVE;
    ch.writerGUID.guidPrefix = msg_info.source_guid_prefix_;
    ch.batch_send = true;
    valid &= CDRMessage::readEntityId(msg, &ch.writerGUID.entityId);

    // Get sequence number
    valid &= CDRMessage::readSequenceNumber(msg, &ch.batchSequenceNumber);

    // Get batch sequence number
    valid &= CDRMessage::readSequenceNumber(msg, &ch.sequenceNumber);

    uint32_t slEncapsulationId, offsetFirstSampleSN;
    // Get batch sequence number
    valid &= CDRMessage::readUInt32(msg, &offsetFirstSampleSN);
    valid &= CDRMessage::readUInt32(msg, &ch.batchSampleCount);
    valid &= CDRMessage::readUInt32(msg, &slEncapsulationId);

    if (!valid) {
        return false;
    }

    if ((ch.sequenceNumber <= SequenceNumber_t()) || (ch.batchSampleCount == 0U)) {
        logWarning(RTPS_MSG_IN, IDSTRING "Invalid message received, bad sequence Number");
        return false;
    }

    uint32_t inlineQosSize = 0U;
    if (inlineQosFlag) {
        if (!ParameterList::updateCacheChangeFromInlineQos(ch, msg, inlineQosSize)) {
            return false;
        }
    }

    uint32_t payload_size;
    payload_size = smh->submessageLength - (RTPSMESSAGE_SAMPLE_INFO_SIZE + octetsToInlineQos + inlineQosSize);
    if (keyFlag) {
        if (payload_size <= 0U) {
            logWarning(RTPS_MSG_IN, IDSTRING "Serialized Payload value invalid (" << payload_size << ")");
            return false;
        }
        /*hanjie: key message size=
         * PARAMETER_STATUS_SIZE+PARAMETER_KEY_SIZE+PARAMETER_SENTINEL_SIZE+2+2*/
        if (payload_size <= 36U) {
            (void)memcpy(ch.instanceHandle.value, &msg->buffer[msg->pos], static_cast<uint64_t>(payload_size));
            ch.serializedPayload.data = &msg->buffer[msg->pos];
            ch.serializedPayload.length = payload_size;
            ch.serializedPayload.max_size = payload_size;
        } else {
            logWarning(RTPS_MSG_IN,
                       IDSTRING "Ignoring Serialized Payload for too large key-only data (" << payload_size << ")");
        }
        msg->pos += payload_size;
    } else {
        uint32_t next_pos = msg->pos + payload_size;
        if ((msg->length >= next_pos) && (payload_size > 0U)) {
            ch.serializedPayload.data = &msg->buffer[msg->pos];
            ch.serializedPayload.length = payload_size;
            ch.serializedPayload.max_size = payload_size;
            msg->pos = next_pos;
        } else {
            logWarning(RTPS_MSG_IN, IDSTRING
                                        "Serialized Payload value invalid or larger than maximum allowed size"
                                        "("
                                        << payload_size << "/" << (msg->length - msg->pos) << ")");
            return false;
        }
    }

    // Set sourcetimestamp
    if (msg_info.have_timestamp_) {
        ch.sourceTimestamp = msg_info.timestamp_;
    }

    // Look for the correct reader to add the change

    process_data_batch_message_function_(ch.readerGUID.entityId, ch);

    // TODO(Ricardo) If an exception is thrown (ex, by ecdr), these lines are not executed ->
    // segmentation fault
    ch.serializedPayload.data = nullptr;
    return true;
#else
    (void)msg;
    (void)smh;
    (void)msg_info;
    elogError(RTPS_HISTORY, RetCode_t::RETCODE_NOT_ENABLED, "Compile macro BATCH_SEND_ENABLE off.");
    return false;
#endif
}

bool MessageReceiver::proc_Submsg_DataFrag(CDRMessage_t* msg, SubmessageHeader_t* smh,
                                           struct MessageInfo& msg_info) const {
    vbs::shared_lock<vbs::shared_mutex> guard(mtx_);

    // READ and PROCESS
    if (smh->submessageLength < RTPSMESSAGE_DATA_MIN_LENGTH) {
        return false;
    }

    // Fill flags bool values
    bool endiannessFlag = (smh->flags & BIT(0)) != 0U;
    bool inlineQosFlag = (smh->flags & BIT(1)) != 0U;
    bool keyFlag = (smh->flags & BIT(2)) != 0U;

    // Assign message endianness
    if (endiannessFlag) {
        msg->msg_endian = LITTLEEND;
    } else {
        msg->msg_endian = BIGEND;
    }

    bool valid = true;

    // Extra flags don't matter now. Avoid those bytes
    msg->pos += 2U;

    int16_t octetsToInlineQos = 0;
    valid &= CDRMessage::readInt16(msg, &octetsToInlineQos);  // it should be 16 in this implementation

    CacheChange_t ch;
    // reader and writer ID
    RTPSReader* first_reader = nullptr;
    valid &= CDRMessage::readEntityId(msg, &ch.readerGUID.entityId);

    // WE KNOW THE READER THAT THE MESSAGE IS DIRECTED TO SO WE LOOK FOR IT:
    if (!willAReaderAcceptMsgDirectedTo(ch.readerGUID.entityId, first_reader)) {
        logDebug(RTPS_MSG_IN, "Process submsg data error(no reader) from " << ch.writerGUID << " to " << ch.readerGUID);
        return false;
    }

    // FOUND THE READER.
    // We ask the reader for a cachechange to store the information.
    ch.writerGUID.guidPrefix = msg_info.source_guid_prefix_;
    valid &= CDRMessage::readEntityId(msg, &ch.writerGUID.entityId);

    // Get sequence number
    valid &= CDRMessage::readSequenceNumber(msg, &ch.sequenceNumber);

    if (ch.sequenceNumber <= SequenceNumber_t()) {
        logDebug(RTPS_MSG_IN, IDSTRING "Invalid message received, bad sequence Number");
        return false;
    }
#if HAVE_SECURITY
    GUID_t readerGUID;
    readerGUID.guidPrefix = msg_info.dest_guid_prefix_;
    if (!checkAuthToken(ch.writerGUID, readerGUID, msg_info, ch.sequenceNumber.to64long())) {
        return false;
    }
#endif

    // READ FRAGMENT NUMBER
    uint32_t fragmentStartingNum;
    valid &= CDRMessage::readUInt32(msg, &fragmentStartingNum);

    // READ FRAGMENTSINSUBMESSAGE
    uint16_t fragmentsInSubmessage = 0U;
    valid &= CDRMessage::readUInt16(msg, &fragmentsInSubmessage);

    // READ FRAGMENTSIZE
    uint16_t fragmentSize = 0U;
    valid &= CDRMessage::readUInt16(msg, &fragmentSize);

    // READ SAMPLESIZE
    uint32_t sampleSize;
    valid &= CDRMessage::readUInt32(msg, &sampleSize);

    if (!valid) {
        return false;
    }

    uint32_t inlineQosSize = 0U;
    if (inlineQosFlag) {
        if (!ParameterList::updateCacheChangeFromInlineQos(ch, msg, inlineQosSize)) {
            return false;
        }
        ch.inline_qos.data = &msg->buffer[msg->pos - inlineQosSize];
        ch.inline_qos.max_size = inlineQosSize;
        ch.inline_qos.length = inlineQosSize;
        ch.inline_qos.encapsulation = endiannessFlag ? PL_CDR_LE : PL_CDR_BE;
        ch.inline_qos.pos = 0;
    }

    uint32_t payload_size;
    payload_size = smh->submessageLength - (RTPSMESSAGE_DATA_EXTRA_INLINEQOS_SIZE + octetsToInlineQos + inlineQosSize);

    // Validations??? XXX TODO

    if (!keyFlag) {
        uint32_t next_pos = msg->pos + payload_size;
        if ((msg->length >= next_pos) && (payload_size > 0U)) {
            ch.kind = ALIVE;
            ch.serializedPayload.data = &msg->buffer[msg->pos];
            ch.serializedPayload.length = payload_size;
            ch.serializedPayload.max_size = payload_size;
            ch.setFragmentSize(fragmentSize);

            msg->pos = next_pos;
        } else {
            logWarning(RTPS_MSG_IN, IDSTRING
                                        "Serialized Payload value invalid or larger than maximum allowed size "
                                        "("
                                        << payload_size << "/" << (msg->length - msg->pos) << ")");
            return false;
        }
    } else {
        /* XXX TODO
           Endianness_t previous_endian = msg->msg_endian;
           if (ch->serializedPayload.encapsulation == PL_CDR_BE)
           msg->msg_endian = BIGEND;
           else if (ch->serializedPayload.encapsulation == PL_CDR_LE)
           msg->msg_endian = LITTLEEND;
           else
           {
           elogError(RTPS_MSG_IN, RetCode_t::RETCODE_ERROR,
                    IDSTRING"Bad encapsulation for KeyHash and status parameter list");
           return false;
           }
           //uint32_t param_size;
           if (ParameterList::readParameterListfromCDRMsg(msg, &m_ParamList, ch, false) <= 0)
           {
           logDebug(RTPS_MSG_IN, IDSTRING"SubMessage Data ERROR, keyFlag ParameterList");
           return false;
           }
           msg->msg_endian = previous_endian;
         */
    }

    // Set sourcetimestamp
    if (msg_info.have_timestamp_) {
        ch.sourceTimestamp = msg_info.timestamp_;
    }

    ch.e2e_header = msg_info.e2e_profile04_;

    process_data_fragment_message_function_(ch.readerGUID.entityId, ch, sampleSize, fragmentStartingNum,
                                            fragmentsInSubmessage);
    ch.serializedPayload.data = nullptr;
    ch.inline_qos.data = nullptr;
    return true;
}

bool MessageReceiver::proc_Submsg_Heartbeat(CDRMessage_t* msg, SubmessageHeader_t* smh,
                                            struct MessageInfo& msg_info) const {
    vbs::shared_lock<vbs::shared_mutex> guard(mtx_);

    bool endiannessFlag = (smh->flags & BIT(0)) != 0U;
    bool finalFlag = (smh->flags & BIT(1)) != 0U;
    bool livelinessFlag = (smh->flags & BIT(2)) != 0U;
    // Assign message endianness
    if (endiannessFlag) {
        msg->msg_endian = LITTLEEND;
    } else {
        msg->msg_endian = BIGEND;
    }

    GUID_t readerGUID;
    GUID_t writerGUID;
    readerGUID.guidPrefix = msg_info.dest_guid_prefix_;
    (void)CDRMessage::readEntityId(msg, &readerGUID.entityId);
    writerGUID.guidPrefix = msg_info.source_guid_prefix_;
    (void)CDRMessage::readEntityId(msg, &writerGUID.entityId);
    SequenceNumber_t firstSN;
    SequenceNumber_t lastSN;
    (void)CDRMessage::readSequenceNumber(msg, &firstSN);
    (void)CDRMessage::readSequenceNumber(msg, &lastSN);

    SequenceNumber_t zeroSN;
    if (firstSN <= zeroSN) {
        logDebug(RTPS_MSG_IN, IDSTRING "Invalid Heartbeat received (" << firstSN << " <= 0), ignoring");
        return false;
    }
    if (lastSN < firstSN && lastSN != firstSN - 1) {
        logDebug(RTPS_MSG_IN, IDSTRING "Invalid Heartbeat received (" << firstSN << ") - (" << lastSN << "), ignoring");
        return false;
    }
    uint32_t HBCount;
    if (!CDRMessage::readUInt32(msg, &HBCount)) {
        logDebug(RTPS_MSG_IN, IDSTRING "Unable to read heartbeat count from heartbeat message");
        return false;
    }
#if HAVE_SECURITY
    if (!checkAuthToken(writerGUID, readerGUID, msg_info, HBCount)) {
        return false;
    }
#endif

    // Look for the correct reader and writers:
    findAllReaders(
        readerGUID.entityId, [&writerGUID, &HBCount, &firstSN, &lastSN, finalFlag, livelinessFlag](RTPSReader* reader) {
            (void)reader->processHeartbeatMsg(writerGUID, HBCount, firstSN, lastSN, finalFlag, livelinessFlag);
        });

    return true;
}

bool MessageReceiver::proc_Submsg_Acknack(CDRMessage_t* msg, SubmessageHeader_t* smh,
                                          struct MessageInfo& msg_info) const {
    vbs::shared_lock<vbs::shared_mutex> guard(mtx_);

    bool endiannessFlag = (smh->flags & BIT(0)) != 0U;
    bool finalFlag = (smh->flags & BIT(1)) != 0U;
    // Assign message endianness
    if (endiannessFlag) {
        msg->msg_endian = LITTLEEND;
    } else {
        msg->msg_endian = BIGEND;
    }
    GUID_t readerGUID;
    GUID_t writerGUID;
    readerGUID.guidPrefix = msg_info.source_guid_prefix_;
    (void)CDRMessage::readEntityId(msg, &readerGUID.entityId);
    writerGUID.guidPrefix = msg_info.dest_guid_prefix_;
    (void)CDRMessage::readEntityId(msg, &writerGUID.entityId);

    SequenceNumberSet_t SNSet = CDRMessage::readSequenceNumberSet(msg);
    uint32_t Ackcount;
    if (!CDRMessage::readUInt32(msg, &Ackcount)) {
        logDebug(RTPS_MSG_IN, IDSTRING "Unable to read ackcount from message");
        return false;
    }
#if HAVE_SECURITY
    if (!checkAuthToken(readerGUID, writerGUID, msg_info, Ackcount)) {
        return false;
    }
#endif

    // Look for the correct writer to use the acknack
    for (RTPSWriter* it : associated_writers_) {
        bool result;
        if (it->process_acknack(writerGUID, readerGUID, Ackcount, SNSet, finalFlag, result)) {
            if (!result) {
                logDebug(RTPS_MSG_IN, IDSTRING "Acknack msg to NOT stateful writer ");
            }
            return result;
        }
    }
    logDebug(RTPS_MSG_IN, IDSTRING "Acknack msg to UNKNOWN writer (I looked through "
                              << associated_writers_.size() << " writers in this ListenResource)");
    return false;
}

bool MessageReceiver::proc_Submsg_Gap(CDRMessage_t* msg, SubmessageHeader_t* smh, struct MessageInfo& msg_info) const {
    vbs::shared_lock<vbs::shared_mutex> guard(mtx_);

    bool endiannessFlag = (smh->flags & BIT(0)) != 0U;
    // Assign message endianness
    if (endiannessFlag) {
        msg->msg_endian = LITTLEEND;
    } else {
        msg->msg_endian = BIGEND;
    }

    GUID_t writerGUID;
    GUID_t readerGUID;
    readerGUID.guidPrefix = msg_info.dest_guid_prefix_;
    (void)CDRMessage::readEntityId(msg, &readerGUID.entityId);
    writerGUID.guidPrefix = msg_info.source_guid_prefix_;
    (void)CDRMessage::readEntityId(msg, &writerGUID.entityId);
#if HAVE_SECURITY
    if (!checkAuthToken(writerGUID, readerGUID, msg_info, participant_->auth_token().max_blocks_per_session + 1)) {
        return false;
    }
#endif
    SequenceNumber_t gapStart;
    (void)CDRMessage::readSequenceNumber(msg, &gapStart);
    SequenceNumberSet_t gapList = CDRMessage::readSequenceNumberSet(msg);
    if (gapStart <= SequenceNumber_t(0, 0U)) {
        return false;
    }

    findAllReaders(readerGUID.entityId, [&writerGUID, &gapStart, &gapList](RTPSReader* reader) {
        (void)reader->processGapMsg(writerGUID, gapStart, gapList);
    });

    return true;
}

bool MessageReceiver::proc_Submsg_InfoTS(CDRMessage_t* msg, SubmessageHeader_t* smh, struct MessageInfo& msg_info) {
    vbs::shared_lock<vbs::shared_mutex> share_lock(mtx_);

    bool endiannessFlag = (smh->flags & BIT(0)) != 0U;
    bool timeFlag = (smh->flags & BIT(1)) != 0U;
    // Assign message endianness
    if (endiannessFlag) {
        msg->msg_endian = LITTLEEND;
    } else {
        msg->msg_endian = BIGEND;
    }
    if (!timeFlag) {
        msg_info.have_timestamp_ = true;
        (void)CDRMessage::readTimestamp(msg, &(msg_info.timestamp_));
    } else {
        msg_info.have_timestamp_ = false;
    }
    return true;
}

#if HAVE_SECURITY
bool MessageReceiver::proc_Submsg_AuthToken(CDRMessage_t* msg, SubmessageHeader_t* smh,
                                            struct MessageInfo& msg_info) const {
    vbs::shared_lock<vbs::shared_mutex> share_lock(mtx_);
    if (smh->submessageLength < 4U) {
        return false;
    }

    bool endiannessFlag = (smh->flags & BIT(0)) != 0U;
    // Assign message endianness
    if (endiannessFlag) {
        msg->msg_endian = LITTLEEND;
    } else {
        msg->msg_endian = BIGEND;
    }

    (void)CDRMessage::readUInt32(msg, &(msg_info.auth_token_));
    return true;
}
bool MessageReceiver::proc_Submsg_SharedKey(CDRMessage_t* msg, SubmessageHeader_t* smh,
                                            struct MessageInfo& msg_info) const {
    vbs::shared_lock<vbs::shared_mutex> share_lock(mtx_);
    if (smh->submessageLength < 32U) {
        return false;
    }

    bool endiannessFlag = (smh->flags & BIT(0)) != 0U;
    // Assign message endianness
    if (endiannessFlag) {
        msg->msg_endian = LITTLEEND;
    } else {
        msg->msg_endian = BIGEND;
    }

    msg_info.shared_key_.resize(32);
    (void)CDRMessage::readData(msg, msg_info.shared_key_.data(), 32U);

    if (participant_->security_manager().is_client) {
        participant_->security_manager().set_shared_key(msg_info.matched_peer_.guidPrefix, msg_info.shared_key_);
    } else if (msg_info.source_guid_prefix_ < participant_->getGuid().guidPrefix) {
        participant_->security_manager().set_shared_key(c_GuidPrefix_Unknown, msg_info.shared_key_);
    }
    return true;
}
#endif

bool MessageReceiver::proc_Submsg_InfoDST(CDRMessage_t* msg, SubmessageHeader_t* smh, struct MessageInfo& msg_info) {
    vbs::shared_lock<vbs::shared_mutex> share_lock(mtx_);

    bool endiannessFlag = (smh->flags & BIT(0)) != 0u;
    // bool timeFlag = smh->flags & BIT(1) ? true : false;
    // Assign message endianness
    if (endiannessFlag) {
        msg->msg_endian = LITTLEEND;
    } else {
        msg->msg_endian = BIGEND;
    }
    GuidPrefix_t guidP;
    (void)CDRMessage::readData(msg, guidP.value, GuidPrefix_t::size);
    if (guidP != c_GuidPrefix_Unknown) {
        msg_info.dest_guid_prefix_ = guidP;
    }

    return true;
}

bool MessageReceiver::proc_Submsg_InfoSRC(CDRMessage_t* msg, SubmessageHeader_t* smh, struct MessageInfo& msg_info) {
    std::lock_guard<vbs::shared_mutex> guard(mtx_);

    bool endiannessFlag = (smh->flags & BIT(0)) != 0U;
    // bool timeFlag = smh->flags & BIT(1) ? true : false;
    // Assign message endianness
    if (endiannessFlag) {
        msg->msg_endian = LITTLEEND;
    } else {
        msg->msg_endian = BIGEND;
    }
    if (smh->submessageLength == INFO_SRC_SUBMSG_LENGTH) {
        // AVOID FIRST 4 BYTES:
        msg->pos += 4U;
        (void)CDRMessage::readOctet(msg, &msg_info.source_version_.m_major);
        (void)CDRMessage::readOctet(msg, &msg_info.source_version_.m_minor);
        (void)CDRMessage::readData(msg, &msg_info.source_vendor_id_[0], 2U);
        (void)CDRMessage::readData(msg, msg_info.source_guid_prefix_.value, GuidPrefix_t::size);
        return true;
    }
    return false;
}

bool MessageReceiver::proc_Submsg_NackFrag(CDRMessage_t* msg, SubmessageHeader_t* smh,
                                           struct MessageInfo& msg_info) const {
    vbs::shared_lock<vbs::shared_mutex> guard(mtx_);

    bool endiannessFlag = (smh->flags & BIT(0)) != 0U;
    // Assign message endianness
    if (endiannessFlag) {
        msg->msg_endian = LITTLEEND;
    } else {
        msg->msg_endian = BIGEND;
    }

    GUID_t readerGUID;
    GUID_t writerGUID;
    readerGUID.guidPrefix = msg_info.source_guid_prefix_;
    (void)CDRMessage::readEntityId(msg, &readerGUID.entityId);
    writerGUID.guidPrefix = msg_info.dest_guid_prefix_;
    (void)CDRMessage::readEntityId(msg, &writerGUID.entityId);

    SequenceNumber_t writerSN;
    (void)CDRMessage::readSequenceNumber(msg, &writerSN);

    FragmentNumberSet_t fnState;
    (void)CDRMessage::readFragmentNumberSet(msg, &fnState);

    uint32_t Ackcount;
    if (!CDRMessage::readUInt32(msg, &Ackcount)) {
        logDebug(RTPS_MSG_IN, IDSTRING "Unable to read ackcount from message");
        return false;
    }
#if HAVE_SECURITY
    if (!checkAuthToken(readerGUID, writerGUID, msg_info, Ackcount)) {
        return false;
    }
#endif

    // Look for the correct writer to use the acknack
    for (RTPSWriter* it : associated_writers_) {
        bool result;
        if (it->process_nack_frag(writerGUID, readerGUID, Ackcount, writerSN, fnState, result)) {
            if (!result) {
                logDebug(RTPS_MSG_IN, IDSTRING "Acknack msg to NOT stateful writer ");
            }
            return result;
        }
    }
    logDebug(RTPS_MSG_IN, IDSTRING "Acknack msg to UNKNOWN writer (I looked through "
                              << associated_writers_.size() << " writers in this ListenResource)");
    return false;
}

bool MessageReceiver::proc_Submsg_HeartbeatFrag(CDRMessage_t* msg, SubmessageHeader_t* smh,
                                                struct MessageInfo& msg_info) const {
    vbs::shared_lock<vbs::shared_mutex> guard(mtx_);

    bool endiannessFlag = (smh->flags & BIT(0)) != 0U;
    // Assign message endianness
    if (endiannessFlag) {
        msg->msg_endian = LITTLEEND;
    } else {
        msg->msg_endian = BIGEND;
    }

    GUID_t readerGUID;
    GUID_t writerGUID;
    readerGUID.guidPrefix = msg_info.dest_guid_prefix_;
    (void)CDRMessage::readEntityId(msg, &readerGUID.entityId);
    writerGUID.guidPrefix = msg_info.source_guid_prefix_;
    (void)CDRMessage::readEntityId(msg, &writerGUID.entityId);

    SequenceNumber_t writerSN;
    (void)CDRMessage::readSequenceNumber(msg, &writerSN);

    FragmentNumber_t lastFN;
    (void)CDRMessage::readUInt32(msg, static_cast<uint32_t*>(&lastFN));

    uint32_t HBCount = 1;
    (void)CDRMessage::readUInt32(msg, &HBCount);

    // XXX TODO VALIDATE DATA?

    // Look for the correct reader and writers:
    /* XXX TODO
       std::lock_guard<std::mutex> guard(mtx_);
       for (std::vector<RTPSReader*>::iterator it = associated_readers_.begin();
            it != associated_readers_.end(); ++it)
       {
           if ((*it)->acceptMsgDirectedTo(readerGUID.entityId))
           {
           (*it)->processHeartbeatMsg(writerGUID, HBCount, firstSN, lastSN, finalFlag,
       livelinessFlag);
           }
       }
     */
    return true;
}

bool MessageReceiver::proc_Submsg_InfoE2E(CDRMessage_t* msg, SubmessageHeader_t* smh, struct MessageInfo& msg_info) {
    std::lock_guard<vbs::shared_mutex> guard(mtx_);

    bool endiannessFlag = (smh->flags & BIT(0)) != 0U;

    // Assign message endianness
    if (endiannessFlag) {
        msg->msg_endian = LITTLEEND;
    } else {
        msg->msg_endian = BIGEND;
    }

    (void)CDRMessage::readE2EProfile04Header(msg, &msg_info.e2e_profile04_);

    return true;
}

} /* namespace rtps */
} /* namespace ertps */
} /* namespace evbs */
